"""
Create: JiBingyu 2024-07-15
Description: 该文件是算法的主文件，用于调用算法库中的算法实现，实现多艇编队控制。
"""

import ctypes
import os
import shutil
import threading
from typing import List

import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np

from Function_j import *
from MultiUSVModel import MultiUSVModel
from StructType import *

# 全局变量定义
Task_data = []  # 任务点， taskPath_list并不是每次都有，所以需要一个全局变量来存储
Refe_data = []  # 参考点， referInfo_list并不是每次都有，所以需要一个全局变量来存储
Obs_data = []  # 障碍物，其中obsR和fOBSV与说明书上相反，代码中半径为0速度为200，说明书上半径为200速度为0
USV_data = []  # 实时的USV位置信息
targer_location = []  # 目标位置
plotting = True  # 是否绘图
clean_flag = True  # 不需要保存数据，方便Push代码
# 各艇控制指令
multiUSVInitInfo: List[USVInitStructType] = []
time_time = 0  # 记录运行时间，存放在Time.txt中
source_dir = "test/test1"


copy_files(source_dir, "config")
clear_dataRecord_folder()


def algorithmImpl(
    algIterNum,
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    global targer_location

    multiUSVStatus_list = Get_USV_data(
        taskPath_data,
        taskPath_size,
        referInfo_data,
        referInfo_size,
        curOBSFusion_data,
        curOBSFusion_size,
        multiUSVStatus_data,
        multiUSVStatus_size,
    )

    #################################### 航点优化部分 ###############################

    # 初始化目标位置
    if len(targer_location) == 0:
        targer_location = np.zeros((5, len(Refe_data), 2))
    # 参考ID和参考点的XY坐标,选择主舰的航点
    refer_ID = [info[2] for info in Refe_data if info[-1] == 1]
    refer_location = [
        [
            Go_to_xy2(
                multiUSVStatus_list[indx].dbCurX,
                multiUSVStatus_list[indx].dbCurY,
                Task_data[-1][0],
                Task_data[-1][1],
            ),
            [multiUSVStatus_list[indx].dbCurX, multiUSVStatus_list[indx].dbCurY],
        ]
        for indx, info in enumerate(multiUSVStatus_list)
        if info.iUSVId in refer_ID
    ][0]

    ##################################################################################
    # TODO: 在此处添加相关控制逻辑代码
    # 根据实时位置，任务信息求下一时刻艇的期望航速航向
    multiUSVInitInfo: List[USVInitStructType] = []

    for i, it in enumerate(multiUSVStatus_list):
        updateUSVControl = USVInitStructType()
        updateUSVControl.iUSVId = it.iUSVId
        updateUSVControl.dbInitX = it.dbCurX
        updateUSVControl.dbInitY = it.dbCurY

        #################################### 控制算法部分 ###############################

        indices = [
            index for index, info in enumerate(Refe_data) if info[2] == it.iUSVId
        ]
        if len(indices):  # 检查是否至少有一个匹配
            # # 航点法
            # targer_location[0][i], targer_location[1][i] = Algorithm_1(
            #     i, multiUSVStatus_list, refer_location, Refe_data, indices
            # )

            # 人工势场法
            (
                targer_location[0][i],
                targer_location[1][i],
                targer_location[2][i],
                targer_location[3][i],
                targer_location[4][i],
                Flag_collision,
            ) = Algorithm_2(
                i,
                multiUSVStatus_list,
                refer_location,
                Refe_data,
                indices,
            )
            if it.iUSVId in refer_ID:
                # 控制主舰的速度和角度
                updateUSVControl.fInitVel, updateUSVControl.fInitYaw = Go_to_xy(
                    it.dbCurX,
                    it.dbCurY,
                    targer_location[1][i][0],
                    targer_location[1][i][1],
                    [3, 10],
                )
            else:
                if Flag_collision:
                    updateUSVControl.fInitVel, updateUSVControl.fInitYaw = Go_to_xy(
                        it.dbCurX,
                        it.dbCurY,
                        targer_location[1][i][0],
                        targer_location[1][i][1],
                        [3, 15],
                    )
                else:
                    updateUSVControl.fInitVel, updateUSVControl.fInitYaw = Go_to_xy(
                        it.dbCurX,
                        it.dbCurY,
                        targer_location[1][i][0],
                        targer_location[1][i][1],
                        [3, 15],
                    )

        ########################################## End #################################
        else:
            # 处理没有找到匹配的情况
            print("No matching USVId found.")

        multiUSVInitInfo.append(updateUSVControl)
    return multiUSVInitInfo
    ##################################################################################


def Get_USV_data(
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    taskPath_list: List[SFormationTaskInfo] = [
        taskPath_data[index] for index in range(taskPath_size.value)
    ]
    referInfo_list: List[referUSVInfo] = [
        referInfo_data[index] for index in range(referInfo_size.value)
    ]
    curOBSFusion_list: List[InputOBSStruct] = [
        curOBSFusion_data[index] for index in range(curOBSFusion_size.value)
    ]
    multiUSVStatus_list: List[USVOutPutStructType] = [
        multiUSVStatus_data[index] for index in range(multiUSVStatus_size.value)
    ]
    Task_data = [] if len(taskPath_list) != 0 else Task_data
    Refe_data = [] if len(referInfo_list) != 0 else Refe_data
    Obs_data = [] if len(curOBSFusion_list) != 0 else Obs_data
    USV_data = [] if len(multiUSVStatus_list) != 0 else USV_data
    for it in taskPath_list:
        Task_data.append([it.referX, it.referY])
    for it in referInfo_list:
        Refe_data.append([it.relatDis, it.relatYaw, it.USVID, it.type])
    for it in curOBSFusion_list:
        # 这里的 obsR 和 fOBSV与说明书上反的
        Obs_data.append([it.dOBSX, it.dOBSY, it.fOBSV])
    for it in multiUSVStatus_list:
        USV_data.append([it.dbCurX, it.dbCurY])

    return multiUSVStatus_list


def plot_graph():
    global plotting
    global Task_data
    global Obs_data
    global USV_data
    global multiUSVInitInfo
    global targer_location
    plt.ioff()  # 关闭交互模式
    while plotting:
        plt.clf()  # 清除之前的图表
        # 目标点
        # plt.scatter([x for x, _ in Task_data], [y for _, y in Task_data], color="blue")
        # for i, (x, y) in enumerate(Task_data):
        #     plt.text(x, y, f"Task{i}", color="blue", fontsize=8)
        #     circle = patches.Circle((x, y), 50, fill=False, edgecolor="blue")
        #     plt.gca().add_patch(circle)

        # 障碍物
        # plt.scatter(
        #     [x for x, _, _ in Obs_data], [y for _, y, _ in Obs_data], color="green"
        # )
        # for i, (x, y, r) in enumerate(Obs_data):
        #     plt.text(x, y, f"Obs{i+1}", color="green", fontsize=8)
        #     circle = patches.Circle((x, y), r, fill=False, edgecolor="green")
        #     plt.gca().add_patch(circle)

        # 每次的航点
        plt.scatter(
            [x for x, _ in targer_location[0]],
            [y for _, y in targer_location[0]],
            color="blue",
        )
        plt.scatter(
            [x for x, _ in targer_location[1]],
            [y for _, y in targer_location[1]],
            color="red",
        )

        # 绘制箭头
        for i, data in enumerate(targer_location[1]):
            dx = data[0] - multiUSVInitInfo[i].dbInitX
            dy = data[1] - multiUSVInitInfo[i].dbInitY
            plt.arrow(
                multiUSVInitInfo[i].dbInitX,
                multiUSVInitInfo[i].dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="pink",
                ec="pink",
            )
        for i, data in enumerate(targer_location[2]):
            dx = data[0] - multiUSVInitInfo[i].dbInitX
            dy = data[1] - multiUSVInitInfo[i].dbInitY
            plt.arrow(
                multiUSVInitInfo[i].dbInitX,
                multiUSVInitInfo[i].dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="green",
                ec="green",
            )
        for i, data in enumerate(targer_location[3]):
            dx = data[0] - multiUSVInitInfo[i].dbInitX
            dy = data[1] - multiUSVInitInfo[i].dbInitY
            plt.arrow(
                multiUSVInitInfo[i].dbInitX,
                multiUSVInitInfo[i].dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="purple",
                ec="purple",
            )
        for i, data in enumerate(targer_location[4]):
            dx = data[0] - multiUSVInitInfo[i].dbInitX
            dy = data[1] - multiUSVInitInfo[i].dbInitY
            plt.arrow(
                multiUSVInitInfo[i].dbInitX,
                multiUSVInitInfo[i].dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="red",
                ec="red",
            )

        # plt.scatter(
        #     [x for x, _ in targer_location[2]],
        #     [y for _, y in targer_location[2]],
        #     color="green",
        # )
        # plt.scatter(
        #     [x for x, _ in targer_location[3]],
        #     [y for _, y in targer_location[3]],
        #     color="purple",
        # )

        # USV位置
        plt.scatter([x for x, _ in USV_data], [y for _, y in USV_data], color="red")
        for i, (x, y) in enumerate(USV_data):
            plt.text(x, y, f"USV{i+1}", color="red", fontsize=8)
            circle = patches.Circle((x, y), 50, fill=False, edgecolor="red")
            plt.gca().add_patch(circle)

        for i, updateUSVControl in enumerate(multiUSVInitInfo):
            dx = updateUSVControl.fInitVel * np.cos(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            dy = updateUSVControl.fInitVel * np.sin(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            plt.arrow(
                updateUSVControl.dbInitX,
                updateUSVControl.dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="blue",
                ec="blue",
            )

        # 添加图例
        plt.legend()
        # 强制设置坐标轴范围

        plt.xlabel("X")
        plt.ylabel("Y")
        plt.title("示例图表")
        plt.axis("equal")  # 确保x和y轴的刻度一致，使得圆形看起来是圆的
        plt.pause(0.01)  # 重新绘制图表

    plt.close("all")


if __name__ == "__main__":
    os.add_dll_directory(os.path.join(os.getcwd(), "./dll"))
    cmultilUSV = MultiUSVModel(
        dll_path=r"./dll/usvmodel.dll",
        dll_deps=[os.path.join(r"./dll", dll_i) for dll_i in os.listdir(r"./dll")],
    )
    # 创建并启动绘图线程
    plot_thread = threading.Thread(target=plot_graph)
    plot_thread.start()

    # 态势信息
    curOBSFusion_data = ctypes.POINTER(InputOBSStruct)()  # 指向InputOBSStruct类型的指针
    curOBSFusion_size = ctypes.c_int()  # 障碍物数量
    # 导航信息
    multiUSVStatus_data = ctypes.POINTER(USVOutPutStructType)()
    multiUSVStatus_size = ctypes.c_int()

    isEnd_int = ctypes.c_int()
    # 任务航路
    taskPath_data = ctypes.POINTER(SFormationTaskInfo)()
    taskPath_size = ctypes.c_int()
    # 构型信息
    referInfo_data = ctypes.POINTER(referUSVInfo)()
    referInfo_size = ctypes.c_int()
    # 整个任务迭代次数
    iterNum = 0
    # 控制模式，1 - 航速航向；2 - 航路，建议使用航速航向
    flagModel = 1
    # 算法更新后清零
    algIterNum = 0

    # 初始化
    cmultilUSV.multiInitialize(flagModel)
    # time_time = time.time()
    # # 生成Time.txt文件
    # with open("Time.txt", "w") as f:
    #     f.write(f"Time: {time_time}\n")

    try:
        for i in range(1000 * 20):
            # # 记录每次仿真时间
            # with open("Time.txt", "a") as f:
            #     f.write(f"{time.time() - time_time}\n")
            # 更新控制指令
            cmultilUSV.updateMultiUSVControl(
                (USVInitStructType * len(multiUSVInitInfo))(*multiUSVInitInfo),
                len(multiUSVInitInfo),
                iterNum,
                flagModel,
            )
            # 获取导航信息和态势信息
            cmultilUSV.getMultiNaviObsInfo(
                iterNum,
                ctypes.byref(multiUSVStatus_data),
                ctypes.byref(multiUSVStatus_size),
                ctypes.byref(curOBSFusion_data),
                ctypes.byref(curOBSFusion_size),
            )
            # 获取任务信息
            rcvFlag = cmultilUSV.getTaskInfo(
                ctypes.byref(isEnd_int),
                ctypes.byref(taskPath_data),
                ctypes.byref(taskPath_size),
                ctypes.byref(referInfo_data),
                ctypes.byref(referInfo_size),
                iterNum,
            )
            # 触发任务
            if 1 == rcvFlag:
                if 1 == isEnd_int.value:  # 到达最后一个任务点，不再发送任务
                    cmultilUSV.taskEnd(iterNum, flagModel)
                    break  # 跳出算法循环
                algIterNum = 0  # 新任务，次数清零
            # 算法更新控制指令
            multiUSVInitInfo: List[USVInitStructType] = algorithmImpl(
                algIterNum,
                taskPath_data,
                taskPath_size,
                referInfo_data,
                referInfo_size,
                curOBSFusion_data,
                curOBSFusion_size,
                multiUSVStatus_data,
                multiUSVStatus_size,
            )

            # 迭代次数更新
            iterNum += 1
            algIterNum += 1
            if is_done(USV_data, targer_location, 50):
                # 线程终止
                plotting = False
                break
        # 当主线程的任务完成后，更新标志以停止绘图线程的循环
    except KeyboardInterrupt:
        plotting = False
        # 等待绘图线程结束
        # plot_thread.join()

if clean_flag:
    clear_dataRecord_folder()
